%%
%注意数据存储方式，是组号(关节1/2)- 时间 - 数据类型（加速度速度或者位置）
motor_res = zeros(2,length(1:simu_step),3);
state_info = zeros(2,length(1:simu_step),3);
debug_info = zeros(2,length(1:simu_step),4);
%%

%测试仿真正不正常
a_dj.reset_state(initial_state);

con_value = [0;0];

for i = 1:simu_step
    motor_res(:,i,:) = tor_motor.simu_loop(a_dj,simu_period,con_value);
    state_info(:,i,:) = a_dj.simu_loop(motor_res(:,i,1),simu_period);
end

figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on


%%

a_dj.reset_state(initial_state);
torque_info = zeros(2,length(1:simu_step),3);
for i = 1:simu_step
    torque_info(:,i,:) = a_dj.inverse_dynamic(1,repmat(vel_s_traj_value(i,:),2,1),repmat(vel_s_traj_value(i,:),2,1).*a_dj.n_factor);
end
figure;
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
title("总力矩");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
title("线性");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
title("非线性")
%%

figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,vel_s_traj_value(1:end-1,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('j1','j2','target','Location',"southeast");
title("vel S traj follow  theta");